#include "MainMotorHandler.h"

uint8 u8motor_currentspeed = 0;
uint8 u8motor_targetspeed = 0;

uint8 u8motor_zerocounter = 0;

uint8 u8motor_status = MOTOR_OFF;

void vfnMotor_ConfigurePWM (void)
{                              
    vfnTPM_PrescalerSelect (MOTOR_TIMER, TPM_PRESCALER_4);
    vfnTPM_SetChannelValue (MOTOR_TIMER, MM_PRIMARY_PWM, MM_PRIM_PWM_DEFAULT);
    vfnTPM_SetChannelValue (MOTOR_TIMER, MM_SECONDARY_PWM, MM_SEC_PWM_DEFAULT);                        
    vfnTPM_ConfigureCAPWM (MOTOR_TIMER, MM_PRIMARY_PWM, TPM_HIGH_TRUE);
    vfnTPM_ConfigureCAPWM (MOTOR_TIMER, MM_SECONDARY_PWM, TPM_LOW_TRUE);
    vfnTPM_SetModulo (MOTOR_TIMER, 80);    // 160 steps at 250ns each.
    vfnTPM_ClockSelect (MOTOR_TIMER, TPM_BUS_CLOCK);
}

void vfnMotor_ConfigureGPIO (void)
{
    MOTOR_OVR_CURRENT_STATUS_DD = GPIO_INPUT;
    MOTOR_SET_DD = GPIO_OUTPUT;
    MOTOR_OVER_CURRENT_DD = GPIO_INPUT;
}

void vfnMotor_AdjustSpeed (void)
{
    // If motor is ON, then regulate speed, otherwise, keep it at 0
    if (u8motor_status == MOTOR_ON)
    {
        if (MOTOR_OVR_CURRENT_STATUS == MOTOR_STATUS_OK)
        {
            if (u8motor_targetspeed != 0)
            {
                if (u8motor_targetspeed < MOTOR_SPEED_ZERO_LIMIT)
                {
                    u8motor_targetspeed = 0;
                }
                else if (u8motor_targetspeed < MOTOR_MIN_SPEED)
                {
                    u8motor_targetspeed = 20;
                }
                else if (u8motor_targetspeed > MOTOR_MAX_SPEED)
                {
                    u8motor_targetspeed = MOTOR_MAX_SPEED;
                }
                u8motor_zerocounter = 0;
            }
            else
            {
                u8motor_zerocounter++;
                if(u8motor_zerocounter == 20)
                {
                    u8motor_status = MOTOR_OFF;     
                }
            }
            
            if (u8motor_targetspeed > u8motor_currentspeed)
            {
                if ((u8motor_targetspeed - u8motor_currentspeed) > MOTOR_MAX_SPEED_INCREMENT)
                {
                    u8motor_currentspeed += MOTOR_MAX_SPEED_INCREMENT;
                }
                else
                {
                    u8motor_currentspeed = u8motor_targetspeed;
                }    
            }
            else
            {
                u8motor_currentspeed = u8motor_targetspeed;
            }
            
            
            vfnTPM_SetChannelValue (MOTOR_TIMER, MM_PRIMARY_PWM, u8motor_currentspeed);
            vfnTPM_SetChannelValue (MOTOR_TIMER, MM_SECONDARY_PWM, u8motor_currentspeed + MM_WINDOW);
        }
        else
        {
            if (MOTOR_OVER_CURRENT == MOTOR_RECOVERED)
            {
                u8motor_currentspeed = MM_PRIM_PWM_MIN;
                
                vfnTPM_SetChannelValue (MOTOR_TIMER, MM_PRIMARY_PWM, MM_PRIM_PWM_MIN);
                vfnTPM_SetChannelValue (MOTOR_TIMER, MM_SECONDARY_PWM, MM_SEC_PWM_MIN);
                
                MOTOR_SET = 1;
                vfnPulseDelay();
                MOTOR_SET = 0;
                
            }
        }
    }
    else
    {
        u8motor_currentspeed = 0;
        vfnTPM_SetChannelValue (MOTOR_TIMER, MM_PRIMARY_PWM, 0);
        vfnTPM_SetChannelValue (MOTOR_TIMER, MM_SECONDARY_PWM, 0 + MM_WINDOW);    
    }
}

void vfnMotor_TurnOn (void)
{
	MOTOR_SET = 1;
	MOTOR_SET = 0;
}


